﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using MotionPlanning.MotionPlanningAlgorithms.RDG_Prototype;
using MotionPlanning.MotionPlanningAlgorithms.ioRRT_Prototype;

namespace MotionPlanning.MotionPlanningPrimitives
{
    public class Trajectory
    {
        protected double cost;
        protected List<Node> Nodes;
        protected List<Edge> Edges;

        public Node CloneState(Node x_target)
        {
            Node x_clone = Node.SpawnGraphicNode();
            x_clone.set_Value(new RoboticSystemPlanningNodeData(
                x_target.get_Value().get_q(),
                x_target.get_Value().get_utility_c()
                ));
            List<Edge> parents = x_target.get_Parents();
            if (parents != null)
            {
                Edge e = parents[parents.Count - 1];
                for (int i = 0; i < parents.Count - 2; ++i)
                    x_clone.add_ParentEdge(null);
                x_clone.add_ParentEdge(e);
            }
            return x_clone;
        }

        public Trajectory(Node x_EndState)
        {
            cost = x_EndState.get_Value().get_utility_c();
            Nodes = new List<Node>();
            Edges = new List<Edge>();

            Node x_trace = x_EndState;
            Nodes.Add(x_trace);
            Edges.Add(null);
            List<Edge> Es = x_trace.get_Parents();
            while (Es != null)
            {
                Edge e_parent = Es[Es.Count - 1];
                Edges.Add(e_parent);
                Node x_parent = e_parent.get_entry();
                Nodes.Add(CloneState(x_parent));
                Es = x_parent.get_Parents();
            }
            Nodes.Reverse();
            Edges.Reverse();
        }

        public void SetCost(double cost)
        {
            this.cost = cost;
        }
        public double GetCost()
        {
            return this.cost;
        }

        public List<Node> GetNodes()
        {
            return Nodes;
        }
        public List<Edge> GetEdges()
        {
            return Edges;
        }
    }
}
